#include "nra12_parser.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <termios.h>

#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
 //#define NRA12_DEBUG

#ifdef NRA12_DEBUG
#include <stdio.h>

const char *parser_state[] = {
	"0_UNSYNC",
	"3_GOT_DIST_L",
	"4_GOT_DIST_H",
	"8_GOT_QUALITY"
	"9_GOT_CHECKSUM"
};
#endif

int nra12_parse(char c, char *parserbuf, unsigned *parserbuf_index, NRA12_PARSE_STATE *state, float *dist)
{
	int ret = -1;
	//char *end;
// PX4_INFO("parse");
	switch (*state) {
	case NRA12_PARSE_STATE::STATE12_GOT_END2:
	// PX4_INFO("STATE12_GOT_END2");
	// PX4_INFO("c=%x",c);
		if (c == 0xaa) {
			*state = NRA12_PARSE_STATE::STATE1_GOT_START1;
		} else {
			*state = NRA12_PARSE_STATE::STATE0_UNSYNC;
		}
		break;

	case NRA12_PARSE_STATE::STATE0_UNSYNC:
	// PX4_INFO("STATE0_UNSYNC");
	// PX4_INFO("c=%x",c);
		if (c == 0xaa) {
			*state = NRA12_PARSE_STATE::STATE1_GOT_START1;
		}
		break;

	case NRA12_PARSE_STATE::STATE1_GOT_START1:
	// PX4_INFO("STATE1_GOT_START1");
	// PX4_INFO("c=%x",c);
		if (c == 0xaa) {
			*state = NRA12_PARSE_STATE::STATE2_GOT_START2;

		}
		else
		{
			*state = NRA12_PARSE_STATE::STATE0_UNSYNC;
		}

		break;
	case NRA12_PARSE_STATE::STATE2_GOT_START2:
	// PX4_INFO("STATE2_GOT_START2");
	// PX4_INFO("c=%x",c);
	if(c == 0x0c){
		*state = NRA12_PARSE_STATE::STATE2_GOT_0C;
	}
	else{
	        *state = NRA12_PARSE_STATE::STATE0_UNSYNC;
	}
		break;
	case NRA12_PARSE_STATE::STATE2_GOT_0C:
	// PX4_INFO("STATE2_GOT_0C");
	// PX4_INFO("c=%x",c);
	if(c == 0x07){
		*state = NRA12_PARSE_STATE::STATE2_GOT_07;
	}
	else{
	        *state = NRA12_PARSE_STATE::STATE0_UNSYNC;
	}
		break;
	case NRA12_PARSE_STATE::STATE2_GOT_07:
	// PX4_INFO("STATE2_GOT_07");
	// PX4_INFO("c=%x",c);
	        *state = NRA12_PARSE_STATE::STATE3_GOT_Index;
		break;
	case NRA12_PARSE_STATE::STATE3_GOT_Index:
	// PX4_INFO("STATE3_GOT_Index");
	// PX4_INFO("c=%x",c);
	        *state = NRA12_PARSE_STATE::STATE4_GOT_Res;
		break;
	case NRA12_PARSE_STATE::STATE4_GOT_Res:
	// PX4_INFO("STATE4_GOT_Res");
	// PX4_INFO("c=%x",c);
		parserbuf[1]=c;
	        *state = NRA12_PARSE_STATE::STATE5_GOT_DIST_H;
		break;

	case NRA12_PARSE_STATE::STATE5_GOT_DIST_H:
	// PX4_INFO("STATE5_GOT_DIST_H");
	// PX4_INFO("c=%x",c);
		*state = NRA12_PARSE_STATE::STATE6_GOT_DIST_L;
		parserbuf[2]=c;
		break;

	case NRA12_PARSE_STATE::STATE6_GOT_DIST_L:
	// PX4_INFO("STATE6_GOT_DIST_L");
	// PX4_INFO("c=%x",c);
		*state = NRA12_PARSE_STATE::STATE7_GOT_UNUSE1;
		break;

	case NRA12_PARSE_STATE::STATE7_GOT_UNUSE1:
	// PX4_INFO("STATE7_GOT_UNUSE1");
	// PX4_INFO("c=%x",c);
		*state = NRA12_PARSE_STATE::STATE8_GOT_UNUSE2;
		break;

	case NRA12_PARSE_STATE::STATE8_GOT_UNUSE2:
	//  PX4_INFO("STATE8_GOT_UNUSE2");
	// PX4_INFO("c=%x",c);
		*state = NRA12_PARSE_STATE::STATE9_GOT_UNUSE3;
		break;

	case NRA12_PARSE_STATE::STATE9_GOT_UNUSE3:
	// PX4_INFO("STATE9_GOT_UNUSE3");
	// PX4_INFO("c=%x",c);
		*state = NRA12_PARSE_STATE::STATE10_GOT_UNUSE4;
		break;

	case NRA12_PARSE_STATE::STATE10_GOT_UNUSE4:
	//  PX4_INFO("STATE10_GOT_UNUSE4");
	// PX4_INFO("c=%x",c);
		if(c==0x55)
		{
		*state = NRA12_PARSE_STATE::STATE11_GOT_END1;
		}
		break;
	case NRA12_PARSE_STATE::STATE11_GOT_END1:
	//  PX4_INFO("STATE11_GOT_END1");
	// PX4_INFO("c=%x",c);
		if(c==0x55)
		{
		*state = NRA12_PARSE_STATE::STATE12_GOT_END2;
		unsigned int t1 = parserbuf[2];
			unsigned int t2 = parserbuf[1];
			t2 <<= 8;
			t2 += t1;

			if (t2 < 0xFFFFu) {
				*dist = ((float)t2) / 100;
			}
		//	 PX4_INFO("dist=%lf\n",(double)*dist);
		}
	        else {
			*state = NRA12_PARSE_STATE::STATE0_UNSYNC;
		}

		break;
	}

#ifdef NRA12_DEBUG
	printf("state: NRA12_PARSE_STATE%s\n", parser_state[*state]);
#endif

	return ret;
}
